#include "robtool.h"
#include "jkutil/iniparser.h"
#include "log/zuclog.h"
#include "config.h"

using axisgroup::RobotToolMgr;
using axisgroup::ToolData;

ToolData::ToolData(int id) {}
void ToolData::reset()
{
    memset(&pose_, 0, sizeof(pose_));
    if (id_ == 0)
    {
        name_ = "TCPFLANGE";
    }
    else
    {
        name_ = "TCP" + std::to_string(id_);
    }
    calib.confirmed = false;
    calib.eps = calib.eps_min = calib.eps_max = 0;
    calib.pose.clear();
}
const ZucPose& ToolData::pose() { return pose_; }
std::string ToolData::name() { return name_; }
void ToolData::config_name(std::string n) { name_ = n; }
int ToolData::set_pose(ZucPose p, bool save)
{
    zuclog_info("TOOL_INFO", "Set TOOL %d: %.2f %.2f %.2f %.2f %.2f %.2f", id_, p.tran.x, p.tran.y, p.tran.z, p.a, p.b, p.c);
    pose_ = p;
    is_changed_ = true;
    return 0;
}
int config_tool_manually(ZucPose pose) { return 0; }
int ToolData::data_source()
{
    if (calib.pose.size() == 4)
    {
        return 1;
    }
    else if (calib.pose.size() == 6)
    {
        return 2;
    }
    else
    {
        return 0;
    }
}
int ToolData::calc_tool_pose(std::array<ZucPose, 4> pose, CalibData* result) { return -1; }
int ToolData::calc_tool_pose(std::array<ZucPose, 6> pose, CalibData* result) { return -1; }
ToolData::CalibData ToolData::get_calib_data() { return calib; }
int ToolData::confirm_calib_result(CalibData* result)
{
    if (result != nullptr)
    {
        calib = *result;
    }
    calib.confirmed = true;

    //TODO:save to config file

    return 0;
}

axisgroup::ToolData RobotToolMgr::__robot_tool[RobotToolMgr::MAX_ROBOT_TOOL_NUM];
std::unique_ptr<Util::IniParser> RobotToolMgr::ini_parser_;

ToolData& RobotToolMgr::get_rob_tool(uint32_t id)
{
    if (id >= MAX_ROBOT_TOOL_NUM)
    {
        return __robot_tool[0];
    }
    return __robot_tool[id];
}

int RobotToolMgr::init_rob_tool(const char* filename)
{
    ini_parser_.reset();
    ini_parser_ = std::make_unique<Util::IniParser>(filename);

    for (uint32_t i = 0; i < MAX_ROBOT_TOOL_NUM; i++)
    {
        std::string tag = "TCP.TCP_" + std::to_string(i);
        __robot_tool[i].id_ = i;

        std::vector<double> tcp;
        ini_parser_->get(tag, tcp, {0, 0, 0, 0, 0, 0}, -10000, 10000);
        ZucPose tcp_pose = {};
        tcp_pose.tran.x = tcp[0];
        tcp_pose.tran.y = tcp[1];
        tcp_pose.tran.z = tcp[2];
        tcp_pose.a = tcp[3];
        tcp_pose.b = tcp[4];
        tcp_pose.c = tcp[5];
        __robot_tool[i].set_pose(tcp_pose);

        std::string name;
        tag = "TCP.TCP_NAME_" + std::to_string(i);
        ini_parser_->get(tag, name, "TCP_" + std::to_string(i));
        __robot_tool[i].config_name(name);

        zuclog_info("TOOL_INFO",
                    "Load TOOL %d: %.2f %.2f %.2f %.2f name %s",
                    i,
                    tcp_pose.tran.x,
                    tcp_pose.tran.y,
                    tcp_pose.tran.z,
                    tcp_pose.a,
                    tcp_pose.b,
                    tcp_pose.c,
                    name.c_str());
        __robot_tool[i].is_changed_ = false;
    }
    return 0;
}

void RobotToolMgr::save()
{
    for (uint32_t i = 0; i < MAX_ROBOT_TOOL_NUM; i++)
    {
        if (!__robot_tool[i].is_changed_)
        {
            continue;
        }
        std::string tag = "TCP.TCP_" + std::to_string(i);
        ZucPose pose = __robot_tool[i].pose();
        std::vector<double> tcp = {pose.tran.x, pose.tran.y, pose.tran.z, pose.a, pose.b, pose.c};
        ini_parser_->set(tag, tcp);

        tag = "TCP.TCP_NAME_" + std::to_string(i);
        ini_parser_->set(tag, __robot_tool[i].name_);
        __robot_tool[i].is_changed_ = false;
    }
    ini_parser_->save_ini();
}